#include "PlaneAssociation.h"

PlaneAssociation::PlaneAssociation(){type = plane_association;}

PlaneAssociation::PlaneAssociation(Plane * a, Plane * b, float desc_dist, float limit){
	p1=a;
	p2=b;
	type = plane_association;
	weight = (limit-desc_dist)/limit;
	printf("weight: %f\n",weight);
}

PlaneAssociation::~PlaneAssociation(){}

float PlaneAssociation::compatable(Association *  asso){

	if(asso->type == keypoint_association){
		return -1;
		
	}else if(asso->type == plane_association){
		PlaneAssociation * a = (PlaneAssociation *)asso;
		if(p1 == a->p1 || p2 == a->p2){
			return 0;
		}

		float p1_angle = p1->angle(a->p1);
		float p2_angle = p2->angle(a->p2);
		float diff = fabs(p1_angle - p2_angle);
		/*
		printf("----------------------------------\n");
		printf("p1->nx: %f, p1->ny: %f, p1->nz: %f\n",p1->normal_x,p1->normal_y,p1->normal_z);
		printf("a->p1->nx: %f, a->p1->ny: %f, a->p1->nz: %f\n",a->p1->normal_x,a->p1->normal_y,a->p1->normal_z);
		printf("\n");
		printf("p2->nx: %f, p2->ny: %f, p2->nz: %f\n",p2->normal_x,p2->normal_y,p2->normal_z);
		printf("a->p2->nx: %f, a->p2->ny: %f, a->p2->nz: %f\n",a->p2->normal_x,a->p2->normal_y,a->p2->normal_z);
		printf("p1_angle: %f, p2_angle: %f, diff: %f\n",p1_angle,p2_angle,diff);
		*/
		if(diff < 0.02){
			return 2;
		}else{
			return 1;
		}
	}else{
		return -1;
	}
}
